#pragma once

#include <bsp.h>
/* --------------------------- */
#include "clock.h"
#include "sheriffos.h"

// ------ spi driver ------
namespace bsp {
class SpiDevice {
private:
    SPI_HandleTypeDef* _hspi;

public:
    /**
     * @brief New a SpiDevice
     * @param hspi Spi handler
     * @author CharlesHsu
     */
    explicit SpiDevice(SPI_HandleTypeDef* hspi);

    /**
     * @brief Init the spi device
     * @param config [SPI_InitTypeDef] The default value is {BaudRatePrescaler :
     * SPI_BAUDRATEPRESCALER_8}
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* init(SPI_InitTypeDef config = {.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8});

    /**
     * @brief receive spi data (for DMA)
     * @param buffer Buffer pointer
     * @param len Buffer length
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* receiveDataDMA(uint8_t* buffer, uint16_t len);

    /**
     * @brief receive spi data
     * @param buffer Buffer pointer
     * @param len Buffer length
     * @param timeout [ms] Timeout duration. The default value is 10 ms
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* receiveData(uint8_t* buffer, uint16_t len, uint32_t timeout = 10);

    /**
     * @brief Send spi data (for DMA)
     * @param buffer Buffer pointer
     * @param len Buffer length
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* sendDataDMA(uint8_t* buffer, uint16_t len);

    /**
     * @brief Send spi data
     * @param buffer Buffer pointer
     * @param len Buffer length
     * @param timeout [ms] Timeout duration. The default value is 10 ms
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* sendData(uint8_t* buffer, uint16_t len, uint32_t timeout = 10);

    /**
     * @brief Swap one byte data or command to spi device
     * @param tx_data data to be sent
     * @param rx_data_ptr pointer to reception data buffer
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* swapOneByteData(uint8_t tx_data, uint8_t* rx_data_ptr);

    /**
     * @brief Swap multi data or command to spi address
     * @param command Data to transmit
     * @param rx_data_ptr Pointer to reception data buffer
     * @param length Length of reception data buffer
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* readMultiReg(uint8_t command, uint8_t* rx_data_ptr, uint8_t length);

    /**
     * @brief Swap data or command to spi address (for DMA)
     * @param tx_data_ptr Pointer to the transmission data buffer
     * @param rx_data_ptr Pointer to reception data buffer
     * @param len Length of data buffer
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr, uint16_t len);

    /**
     * @brief Swap data or command to spi address
     * @param tx_data_ptr Pointer to the transmission data buffer
     * @param rx_data_ptr Pointer to reception data buffer
     * @param len Length of data buffer
     * @param timeout [ms] Timeout duration. The default value is 10 ms
     * @author CharlesHsu
     * @return this
     */
    SpiDevice* swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr, uint16_t len, uint32_t timeout = 10);

    ~SpiDevice() = default;
};
} // namespace bsp

namespace bsp::spi::sd {
void WriteByte(SPI_HandleTypeDef* h_spi, uint8_t data, uint32_t time_out);

void WriteBuffer(SPI_HandleTypeDef* h_spi, uint8_t* buffer, uint16_t len, uint32_t time_out);

uint8_t ReadByte(SPI_HandleTypeDef* h_spi, uint32_t time_out);
} // namespace bsp::spi::sd

namespace bsp::spi::bmi {
void SPI_Init(SPI_HandleTypeDef* hspi);

void SPI_Send(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len, uint32_t timeout);

void SPI_Receive(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len, uint32_t timeout);

void SPI_SendDMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len);

void SPI_ReceiveDMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len);

void SPI_Swap(SPI_HandleTypeDef* hspi, uint8_t* pTxData, uint8_t* pRxData, uint16_t len, uint32_t timeout);

void SPI_SwapDMA(SPI_HandleTypeDef* hspi, uint8_t* pTxData, uint8_t* pRxData, uint16_t len);

uint8_t SPI_SwapAbyte(SPI_HandleTypeDef* hspi, uint8_t txdata);

void SPI_ReadMuliReg(SPI_HandleTypeDef* hspi, uint8_t* rx_data, uint8_t len);
} // namespace bsp::spi::bmi

namespace bsp {
class GpioDriver {
private:
    GPIO_TypeDef* _gpio;
    uint16_t _pin;
    GPIO_PinState _state;

public:
    GpioDriver(GPIO_TypeDef* gpio, uint16_t pin, GPIO_PinState state = GPIO_PIN_RESET);

    GpioDriver* reset();

    GpioDriver* set();

    GpioDriver* toggle();

    GPIO_PinState read();

    ~GpioDriver() = default;
};
} // namespace bsp

constexpr auto GRAVITY = units::acceleration::meters_per_second_squared_t(9.7986f);

using namespace units::literals;
using namespace units::acceleration;
using namespace units::angular_velocity;
using namespace units::temperature;
using namespace units::time;
using namespace units::dimensionless;
using namespace units::literals;

constexpr auto BMI088_COM_WAIT_SENSOR_TIME = 2;
constexpr auto BMI088_LONG_DELAY_TIME = 65;

using namespace units::literals;
using namespace units::acceleration;
using namespace units::angle;
using namespace units::angular_velocity;
using namespace units::temperature;
using namespace units::time;
using namespace units::dimensionless;

using AccelStruct = struct {
    meters_per_second_squared_t x;
    meters_per_second_squared_t y;
    meters_per_second_squared_t z;
};
using GyroStruct = struct {
    degrees_per_second_t yaw;
    degrees_per_second_t pitch;
    degrees_per_second_t roll;
};
using ConsequentAngle = struct {
    degree_t limitedAngle;
    scalar_t rounds;
};

enum {
    BMI088_NO_ERROR = 0x00,
    BMI088_ACC_PWR_CTRL_ERROR = 0x01,
    BMI088_ACC_PWR_CONF_ERROR = 0x02,
    BMI088_ACC_CONF_ERROR = 0x03,
    BMI088_ACC_SELF_TEST_ERROR = 0x04,
    BMI088_ACC_RANGE_ERROR = 0x05,
    BMI088_INT1_IO_CTRL_ERROR = 0x06,
    BMI088_INT_MAP_DATA_ERROR = 0x07,
    BMI088_GYRO_RANGE_ERROR = 0x08,
    BMI088_GYRO_BANDWIDTH_ERROR = 0x09,
    BMI088_GYRO_LPM1_ERROR = 0x0A,
    BMI088_GYRO_CTRL_ERROR = 0x0B,
    BMI088_GYRO_INT3_INT4_IO_CONF_ERROR = 0x0C,
    BMI088_GYRO_INT3_INT4_IO_MAP_ERROR = 0x0D,

    BMI088_SELF_TEST_ACCEL_ERROR = 0x80,
    BMI088_SELF_TEST_GYRO_ERROR = 0x40,
    BMI088_NO_SENSOR = 0xFF,
};

#define BMI088_BOARD_INSTALL_SPIN_MATRIX \
    {0.0f, 1.0f, 0.0f}, {-1.0f, 0.0f, 0.0f}, { 0.0f, 0.0f, 1.0f }

#define BMI088_TEMP_FACTOR 0.125f
#define BMI088_TEMP_OFFSET 23.0f

#define BMI088_GxOFFSET 0.00247530174f
#define BMI088_GyOFFSET 0.000393082853f
#define BMI088_GzOFFSET 0.000393082853f
#define BMI088_gNORM 9.69293118f

#define BMI088_WRITE_ACCEL_REG_NUM 6
#define BMI088_WRITE_GYRO_REG_NUM 6

#define BMI088_GYRO_DATA_READY_BIT 0
#define BMI088_ACCEL_DATA_READY_BIT 1
#define BMI088_ACCEL_TEMP_DATA_READY_BIT 2

#define BMI088_ACCEL_IIC_ADDRESSE (0x18 << 1)
#define BMI088_GYRO_IIC_ADDRESSE (0x68 << 1)

#define BMI088_ACCEL_RANGE_3G
#define BMI088_ACCEL_RANGE_6G
#define BMI088_ACCEL_RANGE_12G
#define BMI088_ACCEL_RANGE_24G

#define BMI088_GYRO_RANGE_2000
#define BMI088_GYRO_RANGE_1000
#define BMI088_GYRO_RANGE_500
#define BMI088_GYRO_RANGE_250
#define BMI088_GYRO_RANGE_125

#define BMI088_ACCEL_3G_SEN 0.0008974358974f
#define BMI088_ACCEL_6G_SEN 0.00179443359375f
#define BMI088_ACCEL_12G_SEN 0.0035888671875f
#define BMI088_ACCEL_24G_SEN 0.007177734375f

#define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f
#define BMI088_GYRO_1000_SEN 0.00053263221801584764920766930190693f
#define BMI088_GYRO_500_SEN 0.00026631610900792382460383465095346f
#define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
#define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
#define BMI088_ACC_CHIP_ID 0x00  // the register is  " Who am I "
#define BMI088_ACC_CHIP_ID_VALUE 0x1E
// #define BMI088_ACC_CHIP_ID_VALUE 0xFA

#define BMI088_ACC_ERR_REG 0x02
#define BMI088_ACCEL_CONGIF_ERROR_SHFITS 0x2
#define BMI088_ACCEL_CONGIF_ERROR (1 << BMI088_ACCEL_CONGIF_ERROR_SHFITS)
#define BMI088_FATAL_ERROR_SHFITS 0x0
#define BMI088_FATAL_ERROR (1 << BMI088_FATAL_ERROR)

#define BMI088_ACC_STATUS 0x03
#define BMI088_ACCEL_DRDY_SHFITS 0x7
#define BMI088_ACCEL_DRDY (1 << BMI088_ACCEL_DRDY_SHFITS)

#define BMI088_ACCEL_XOUT_L 0x12
#define BMI088_ACCEL_XOUT_M 0x13
#define BMI088_ACCEL_YOUT_L 0x14
#define BMI088_ACCEL_YOUT_M 0x15
#define BMI088_ACCEL_ZOUT_L 0x16
#define BMI088_ACCEL_ZOUT_M 0x17

#define BMI088_SENSORTIME_DATA_L 0x18
#define BMI088_SENSORTIME_DATA_M 0x19
#define BMI088_SENSORTIME_DATA_H 0x1A

#define BMI088_ACC_INT_STAT_1 0x1D
#define BMI088_ACCEL_DRDY_INTERRUPT_SHFITS 0x7
#define BMI088_ACCEL_DRDY_INTERRUPT (1 << BMI088_ACCEL_DRDY_INTERRUPT_SHFITS)

#define BMI088_TEMP_M 0x22
#define BMI088_TEMP_L 0x23

#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_CONF_MUST_Set 0x80
#define BMI088_ACC_BWP_SHFITS 0x4
#define BMI088_ACC_OSR4 (0x0 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_OSR2 (0x1 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_NORMAL (0x2 << BMI088_ACC_BWP_SHFITS)

#define BMI088_ACC_ODR_SHFITS 0x0
#define BMI088_ACC_12_5_HZ (0x5 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_25_HZ (0x6 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_50_HZ (0x7 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_100_HZ (0x8 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_200_HZ (0x9 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_400_HZ (0xA << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_800_HZ (0xB << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_1600_HZ (0xC << BMI088_ACC_ODR_SHFITS)

#define BMI088_ACC_RANGE 0x41
#define BMI088_ACC_RANGE_SHFITS 0x0
#define BMI088_ACC_RANGE_3G (0x0 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_6G (0x1 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_12G (0x2 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_24G (0x3 << BMI088_ACC_RANGE_SHFITS)

#define BMI088_INT1_IO_CTRL 0x53
#define BMI088_ACC_INT1_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT1_IO_ENABLE (0x1 << BMI088_ACC_INT1_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT1_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT1_GPIO_PP (0x0 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_OD (0x1 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT1_GPIO_LOW (0x0 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT1_GPIO_HIGH (0x1 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)

#define BMI088_INT2_IO_CTRL 0x54
#define BMI088_ACC_INT2_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT2_IO_ENABLE (0x1 << BMI088_ACC_INT2_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT2_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT2_GPIO_PP (0x0 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_OD (0x1 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT2_GPIO_LOW (0x0 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT2_GPIO_HIGH (0x1 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)

#define BMI088_INT_MAP_DATA 0x58
#define BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS 0x6
#define BMI088_ACC_INT2_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS 0x2
#define BMI088_ACC_INT1_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS)

#define BMI088_ACC_SELF_TEST 0x6D
#define BMI088_ACC_SELF_TEST_OFF 0x00
#define BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL 0x0D
#define BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL 0x09

#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_PWR_SUSPEND_MODE 0x03
#define BMI088_ACC_PWR_ACTIVE_MODE 0x00

#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_ENABLE_ACC_OFF 0x00
#define BMI088_ACC_ENABLE_ACC_ON 0x04

#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_SOFTRESET_VALUE 0xB6

#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_CHIP_ID_VALUE 0x0F

#define BMI088_GYRO_X_L 0x02
#define BMI088_GYRO_X_H 0x03
#define BMI088_GYRO_Y_L 0x04
#define BMI088_GYRO_Y_H 0x05
#define BMI088_GYRO_Z_L 0x06
#define BMI088_GYRO_Z_H 0x07

#define BMI088_GYRO_INT_STAT_1 0x0A
#define BMI088_GYRO_DYDR_SHFITS 0x7
#define BMI088_GYRO_DYDR (0x1 << BMI088_GYRO_DYDR_SHFITS)

#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_RANGE_SHFITS 0x0
#define BMI088_GYRO_2000 (0x0 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_1000 (0x1 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_500 (0x2 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_250 (0x3 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_125 (0x4 << BMI088_GYRO_RANGE_SHFITS)

#define BMI088_GYRO_BANDWIDTH 0x10

// the first num means Output data  rate, the second num means bandwidth
#define BMI088_GYRO_BANDWIDTH_MUST_Set 0x80
#define BMI088_GYRO_2000_532_HZ 0x00
#define BMI088_GYRO_2000_230_HZ 0x01
#define BMI088_GYRO_1000_116_HZ 0x02
#define BMI088_GYRO_400_47_HZ 0x03
#define BMI088_GYRO_200_23_HZ 0x04
#define BMI088_GYRO_100_12_HZ 0x05
#define BMI088_GYRO_200_64_HZ 0x06
#define BMI088_GYRO_100_32_HZ 0x07

#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_NORMAL_MODE 0x00
#define BMI088_GYRO_SUSPEND_MODE 0x80
#define BMI088_GYRO_DEEP_SUSPEND_MODE 0x20

#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_SOFTRESET_VALUE 0xB6

#define BMI088_GYRO_CTRL 0x15
#define BMI088_DRDY_OFF 0x00
#define BMI088_DRDY_ON 0x80

#define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
#define BMI088_GYRO_INT4_GPIO_MODE_SHFITS 0x3
#define BMI088_GYRO_INT4_GPIO_PP (0x0 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_OD (0x1 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_LVL_SHFITS 0x2
#define BMI088_GYRO_INT4_GPIO_LOW (0x0 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT4_GPIO_HIGH (0x1 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_MODE_SHFITS 0x1
#define BMI088_GYRO_INT3_GPIO_PP (0x0 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_OD (0x1 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_LVL_SHFITS 0x0
#define BMI088_GYRO_INT3_GPIO_LOW (0x0 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_HIGH (0x1 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)

#define BMI088_GYRO_INT3_INT4_IO_MAP 0x18

#define BMI088_GYRO_DRDY_IO_OFF 0x00
#define BMI088_GYRO_DRDY_IO_INT3 0x01
#define BMI088_GYRO_DRDY_IO_INT4 0x80
#define BMI088_GYRO_DRDY_IO_BOTH (BMI088_GYRO_DRDY_IO_INT3 | BMI088_GYRO_DRDY_IO_INT4)

#define BMI088_GYRO_SELF_TEST 0x3C
#define BMI088_GYRO_RATE_OK_SHFITS 0x4
#define BMI088_GYRO_RATE_OK (0x1 << BMI088_GYRO_RATE_OK_SHFITS)
#define BMI088_GYRO_BIST_FAIL_SHFITS 0x2
#define BMI088_GYRO_BIST_FAIL (0x1 << BMI088_GYRO_BIST_FAIL_SHFITS)
#define BMI088_GYRO_BIST_RDY_SHFITS 0x1
#define BMI088_GYRO_BIST_RDY (0x1 << BMI088_GYRO_BIST_RDY_SHFITS)
#define BMI088_GYRO_TRIG_BIST_SHFITS 0x0
#define BMI088_GYRO_TRIG_BIST (0x1 << BMI088_GYRO_TRIG_BIST_SHFITS)

static uint8_t write_BMI088_accel_reg_data_error[BMI088_WRITE_ACCEL_REG_NUM][3] = {
    {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
    {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
    {BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
    {BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
    {BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW,
     BMI088_INT1_IO_CTRL_ERROR},
    {BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}};

static uint8_t write_BMI088_gyro_reg_data_error[BMI088_WRITE_GYRO_REG_NUM][3] = {
    {BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
    {BMI088_GYRO_BANDWIDTH, BMI088_GYRO_1000_116_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
    {BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
    {BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
    {BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW,
     BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
    {BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}};

static uint8_t const write_BMI088_ACCEL_self_test_Reg_Data_Error[6][3] = {
    {BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_1600_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
    {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
    {BMI088_ACC_RANGE, BMI088_ACC_RANGE_24G, BMI088_ACC_RANGE_ERROR},
    {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
    {BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL, BMI088_ACC_PWR_CONF_ERROR},
    {BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL, BMI088_ACC_PWR_CONF_ERROR}};

class Bmi088 {
public:
    using StateEnum = enum { Null = 0, Connected = 1, Lost = 2, Error = 3, Pending = 4 };

public:
    /* 测量值 */
    AccelStruct accel_measurements{};
    GyroStruct gyro_measurements{};
    celsius_t temperature{0};
    /* 性能观测项 */
    microsecond_t update_dt{0};
    float last_update_tick{0};

private:
    /* 基本设备 */
    bsp::SpiDevice* _spi{nullptr};
    bsp::GpioDriver* _accel_chip_selection_pin{nullptr};
    bsp::GpioDriver* _gyro_chip_selection_pin{nullptr};
    /* 偏置 */
    GyroStruct _gyro_offset{};
    /* 设备状态 */
    StateEnum _state{Null};
    /* 设备参数及常数 */
    meters_per_second_squared_t _g_norm{0};
    meters_per_second_squared_t _accel_sen{0};
    degrees_per_second_t _gyro_sen{0};
    scalar_t _accel_scale{0};

public:
    /**
     * @brief 依赖注入
     * @param [SpiDevice *] spi SPI 设备
     * @param [GpioDevice *] accel_chip_selection_pin 加速度计片选引脚
     * @param [GpioDevice *] gyro_chip_selection_pin 陀螺仪片选引脚
     */
    Bmi088(bsp::SpiDevice* spi, bsp::GpioDriver* accel_chip_selection_pin, bsp::GpioDriver* gyro_chip_selection_pin)
        : _spi{spi},
          _accel_chip_selection_pin{accel_chip_selection_pin},
          _gyro_chip_selection_pin{gyro_chip_selection_pin} {
        reset();
    };

    void setOffset(GyroStruct const& GYRO_Offset, meters_per_second_squared_t const& G_Norm) {
        _gyro_offset = GYRO_Offset;
        _g_norm = G_Norm;
        _accel_scale = GRAVITY / _g_norm;
    }

    Bmi088* init() {
        reset();

        uint8_t error = BMI088_NO_ERROR;
        do {
            _accel_sen = meters_per_second_squared_t(BMI088_ACCEL_6G_SEN);
            _gyro_sen = degrees_per_second_t(BMI088_GYRO_2000_SEN);

            error |= (Bmi088AccelTest(this) != BMI088_NO_ERROR) ? BMI088_SELF_TEST_ACCEL_ERROR : Bmi088AccelInit(this);

            error |= (Bmi088GyroTest(this) != BMI088_NO_ERROR) ? BMI088_SELF_TEST_GYRO_ERROR : Bmi088GyroInit(this);

            // caliOffset(); // 一块板子校准一次即可，之后使用setOffset

            _state = error != BMI088_NO_ERROR ? Lost : Connected;
        } while (error);

        return this;
    };

    Bmi088* update() {
        using namespace bsp;
        uint8_t buff[8] = {0, 0, 0, 0, 0, 0};
        int16_t raw_temp;

        _state = Pending;
        update_dt = os::getDWTTime_u32() - update_dt;
        last_update_tick = os::getDWTTime_u32().to<float>();

        accelReadMultiReg(BMI088_ACCEL_XOUT_L, buff, 6);

        accel_measurements.x = ((int16_t)((buff[1]) << 8) | buff[0]) * _accel_sen * _accel_scale;
        accel_measurements.y = ((int16_t)((buff[3]) << 8) | buff[2]) * _accel_sen * _accel_scale;
        accel_measurements.z = ((int16_t)((buff[5]) << 8) | buff[4]) * _accel_sen * _accel_scale;

        gyroReadMultiReg(BMI088_GYRO_CHIP_ID, buff, 8);
        if (buff[0] == BMI088_GYRO_CHIP_ID_VALUE) {
            gyro_measurements.pitch = ((int16_t)((buff[3]) << 8) | buff[2]) * _gyro_sen - _gyro_offset.pitch;
            gyro_measurements.roll = ((int16_t)((buff[5]) << 8) | buff[4]) * _gyro_sen - _gyro_offset.roll;
            gyro_measurements.yaw = ((int16_t)((buff[7]) << 8) | buff[6]) * _gyro_sen - _gyro_offset.yaw;
        }
        accelReadMultiReg(BMI088_TEMP_M, buff, 2);

        raw_temp = (int16_t)((buff[0] << 3) | (buff[1] >> 5));
        if (raw_temp > 1023) {
            raw_temp -= 2048;
        }

        temperature = celsius_t(raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET);
        _state = Connected;
        return this;
    };

    Bmi088* reset() {
        accel_measurements.x = 0_mps_sq;
        accel_measurements.y = 0_mps_sq;
        accel_measurements.z = 0_mps_sq;
        gyro_measurements.pitch = 0_deg_per_s;
        gyro_measurements.roll = 0_deg_per_s;
        gyro_measurements.yaw = 0_deg_per_s;
        temperature = 0_degC;
        update_dt = 0_ms;
        last_update_tick = 0;
        return this;
    };

    Bmi088* accelWriteSingleReg(uint8_t reg, uint8_t data) {
        uint8_t rxData;
        _accel_chip_selection_pin->reset();
        _spi->swapOneByteData(reg, &rxData)->swapOneByteData(data, &rxData);
        _accel_chip_selection_pin->set();
        return this;
    };

    Bmi088* accelReadSingleReg(uint8_t reg, uint8_t* data_ptr) {
        uint8_t rxData;
        _accel_chip_selection_pin->reset();
        _spi->swapOneByteData(reg | 0x80, &rxData)->swapOneByteData(0x55, &rxData)->swapOneByteData(0x55, data_ptr);
        _accel_chip_selection_pin->set();
        return this;
    };

    Bmi088* accelReadMultiReg(uint8_t reg, uint8_t* data_ptr, uint8_t len) {
        _accel_chip_selection_pin->reset();
        _spi->swapOneByteData(reg | 0x80, data_ptr)
            ->swapOneByteData(reg | 0x80, data_ptr)
            ->readMultiReg(0x55, data_ptr, len);
        _accel_chip_selection_pin->set();
        return this;
    };

    Bmi088* gyroWriteSingleReg(uint8_t reg, uint8_t data) {
        uint8_t rxData;
        _gyro_chip_selection_pin->reset();
        _spi->swapOneByteData(reg, &rxData)->swapOneByteData(data, &rxData);
        _gyro_chip_selection_pin->set();
        return this;
    };

    Bmi088* gyroReadSingleReg(uint8_t reg, uint8_t* data_ptr) {
        uint8_t rxData;
        _gyro_chip_selection_pin->reset();
        _spi->swapOneByteData(reg | 0x80, &rxData)->swapOneByteData(0x55, data_ptr);
        _gyro_chip_selection_pin->set();
        return this;
    };

    Bmi088* gyroReadMultiReg(uint8_t reg, uint8_t* data_ptr, uint8_t len) {
        _gyro_chip_selection_pin->reset();
        _spi->swapOneByteData(reg | 0x80, data_ptr)->readMultiReg(0x55, data_ptr, len);
        _gyro_chip_selection_pin->set();
        return this;
    };

    Bmi088* caliOffset() {
        using namespace bsp;
        static uint16_t cali_times = 6000;
        degrees_per_second_t gyroMax[3], gyroMin[3], gyroDiff[3];
        meters_per_second_squared_t gNormTemp, gNormMax, gNormMin, gNormDiff;
        uint8_t buff[8] = {0, 0, 0, 0, 0, 0};
        int16_t caliCount = 0;
        units::time::second_t start_time = os::getDWTTime_u32();

        _gyro_offset.pitch = degrees_per_second_t(BMI088_GxOFFSET);
        _gyro_offset.roll = degrees_per_second_t(BMI088_GyOFFSET);
        _gyro_offset.yaw = degrees_per_second_t(BMI088_GzOFFSET);
        _g_norm = meters_per_second_squared_t(BMI088_gNORM);

        do {
            if (os::getDWTTime_u32() - start_time > 10_s) {
                _gyro_offset.pitch = degrees_per_second_t(BMI088_GxOFFSET);
                _gyro_offset.roll = degrees_per_second_t(BMI088_GyOFFSET);
                _gyro_offset.yaw = degrees_per_second_t(BMI088_GzOFFSET);
                _g_norm = meters_per_second_squared_t(BMI088_gNORM);
                break;
            }
            os::sleep(5_ms);

            _gyro_offset.pitch = 0.00309869158_deg_per_s;
            _gyro_offset.roll = -0.000884874258_deg_per_s;
            _gyro_offset.yaw = 0.000428945146_deg_per_s;
            _g_norm = 9.71904564_mps_sq;

            for (uint16_t i = 0; i < cali_times; i++) {
                accelReadMultiReg(BMI088_ACCEL_XOUT_L, buff, 6);
                accel_measurements.x = (static_cast<int16_t>((buff[1]) << 8) | buff[0]) * _accel_sen;
                accel_measurements.y = (static_cast<int16_t>((buff[3]) << 8) | buff[2]) * _accel_sen;
                accel_measurements.z = (static_cast<int16_t>((buff[5]) << 8) | buff[4]) * _accel_sen;

                gNormTemp = units::math::sqrt(accel_measurements.x * accel_measurements.x +
                                              accel_measurements.y * accel_measurements.y +
                                              accel_measurements.z * accel_measurements.z);
                _g_norm += gNormTemp;

                gyroReadMultiReg(BMI088_GYRO_CHIP_ID, buff, 8);
                if (buff[0] == BMI088_GYRO_CHIP_ID_VALUE) {
                    gyro_measurements.pitch = (static_cast<int16_t>((buff[3]) << 8) | buff[2]) * _gyro_sen;
                    gyro_measurements.roll = (static_cast<int16_t>((buff[5]) << 8) | buff[4]) * _gyro_sen;
                    gyro_measurements.yaw = (static_cast<int16_t>((buff[7]) << 8) | buff[6]) * _gyro_sen;
                    _gyro_offset.pitch += gyro_measurements.pitch;
                    _gyro_offset.roll += gyro_measurements.roll;
                    _gyro_offset.yaw += gyro_measurements.yaw;
                }

                if (i == 0) {
                    gNormMax = gNormTemp;
                    gNormMin = gNormTemp;
                    gyroMax[0] = gyro_measurements.pitch;
                    gyroMin[0] = gyro_measurements.pitch;
                    gyroMax[1] = gyro_measurements.roll;
                    gyroMin[1] = gyro_measurements.roll;
                    gyroMax[2] = gyro_measurements.yaw;
                    gyroMin[2] = gyro_measurements.yaw;
                } else {
                    GetMaxandMinRange(gNormTemp, gNormMax, gNormMin);
                    GetMaxandMinRange(gyro_measurements.pitch, gyroMax[0], gyroMin[0]);
                    GetMaxandMinRange(gyro_measurements.roll, gyroMax[1], gyroMin[1]);
                    GetMaxandMinRange(gyro_measurements.yaw, gyroMax[2], gyroMin[2]);
                }

                gNormDiff = gNormMax - gNormMin;
                for (uint8_t j = 0; j < 3; j++) {
                    gyroDiff[j] = gyroMax[j] - gyroMin[j];
                }
                if (gNormDiff > 0.7_mps_sq || gyroDiff[0] > 0.15_deg_per_s || gyroDiff[1] > 0.15_deg_per_s ||
                    gyroDiff[2] > 0.15_deg_per_s) {
                    break;
                }
                os::sleep(5_ms);
            }

            _g_norm /= static_cast<float>(cali_times);

            _gyro_offset.pitch /= cali_times;
            _gyro_offset.roll /= cali_times;
            _gyro_offset.yaw /= cali_times;

            caliCount++;
        } while (gNormDiff > 0.7_mps_sq || units::math::fabs(_g_norm - GRAVITY) > 0.5_mps_sq ||
                 gyroDiff[0] > 0.15_deg_per_s || gyroDiff[1] > 0.15_deg_per_s || gyroDiff[2] > 0.15_deg_per_s ||
                 units::math::fabs(_gyro_offset.pitch) > 0.01_deg_per_s ||
                 units::math::fabs(_gyro_offset.roll) > 0.01_deg_per_s ||
                 units::math::fabs(_gyro_offset.yaw) > 0.01_deg_per_s);

        _accel_scale = GRAVITY / _g_norm;
        return this;
    };

    friend uint8_t Bmi088AccelInit(Bmi088* bmi088) {
        uint8_t res = 0;

        // check commiunication
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        // accel software reset
        bmi088->accelWriteSingleReg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
        bsp::delay_ms(BMI088_LONG_DELAY_TIME);

        // check commiunication is normal after reset
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        // check the "who am I"
        if (res != BMI088_ACC_CHIP_ID_VALUE) {
            return BMI088_NO_SENSOR;
        }

        // set accel sonsor config and check
        for (auto& write_reg_num : write_BMI088_accel_reg_data_error) {
            bmi088->accelWriteSingleReg(write_reg_num[0], write_reg_num[1]);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            bmi088->accelReadSingleReg(write_reg_num[0], &res);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            if (res != write_reg_num[1]) {
                return write_reg_num[2];
            }
        }
        return BMI088_NO_ERROR;
    };

    friend uint8_t Bmi088GyroInit(Bmi088* bmi088) {
        uint8_t res = 0;

        // check commiunication
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        // reset the gyro sensor
        bmi088->gyroWriteSingleReg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
        bsp::delay_ms(BMI088_LONG_DELAY_TIME);
        // check commiunication is normal after reset
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        // check the "who am I"
        if (res != BMI088_GYRO_CHIP_ID_VALUE) {
            return BMI088_NO_SENSOR;
        }

        // set gyro sonsor config and check
        for (auto& write_reg_num : write_BMI088_gyro_reg_data_error) {
            bmi088->gyroWriteSingleReg(write_reg_num[0], write_reg_num[1]);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            bmi088->gyroReadSingleReg(write_reg_num[0], &res);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            if (res != write_reg_num[1]) {
                return write_reg_num[2];
            }
        }

        return BMI088_NO_ERROR;
    };

    friend uint8_t Bmi088AccelTest(Bmi088* bmi088) {
        uint8_t res = 0;
        int16_t self_test_accel[2][3];
        uint8_t buff[6] = {0, 0, 0, 0, 0, 0};

        // check commiunication is normal
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        // reset  bmi088 accel sensor and wait for > 50ms
        bmi088->accelWriteSingleReg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
        bsp::delay_ms(BMI088_LONG_DELAY_TIME);

        // check commiunication is normal
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        if (res != BMI088_ACC_CHIP_ID_VALUE) {
            return BMI088_NO_SENSOR;
        }

        // set the accel register
        for (int write_reg_num = 0; write_reg_num < 4; write_reg_num++) {
            bmi088->accelWriteSingleReg(write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][0],
                                        write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][1]);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            bmi088->accelReadSingleReg(write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][0], &res);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            if (res != write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][1]) {
                return write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][2];
            }

            // accel conf and accel range  . the two register set need wait for > 50ms
            bsp::delay_ms(BMI088_LONG_DELAY_TIME);
        }

        // self test include postive and negative
        for (int write_reg_num = 0; write_reg_num < 2; write_reg_num++) {
            bmi088->accelWriteSingleReg(write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][0],
                                        write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][1]);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            bmi088->accelReadSingleReg(write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][0], &res);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

            if (res != write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][1]) {
                return write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][2];
            }

            // accel conf and accel range  . the two register set need wait for > 50ms
            bsp::delay_ms(BMI088_LONG_DELAY_TIME);

            // read response accel
            bmi088->accelReadMultiReg(BMI088_ACCEL_XOUT_L, buff, 6);
            self_test_accel[write_reg_num][0] = static_cast<int16_t>((buff[1] << 8) | buff[0]);
            self_test_accel[write_reg_num][1] = static_cast<int16_t>((buff[3] << 8) | buff[2]);
            self_test_accel[write_reg_num][2] = static_cast<int16_t>((buff[5] << 8) | buff[4]);
        }

        // set self test off
        bmi088->accelWriteSingleReg(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_OFF);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->accelReadSingleReg(BMI088_ACC_SELF_TEST, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        if (res != (BMI088_ACC_SELF_TEST_OFF)) {
            return BMI088_ACC_SELF_TEST_ERROR;
        }

        // reset the accel sensor
        bmi088->accelWriteSingleReg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
        bsp::delay_ms(BMI088_LONG_DELAY_TIME);

        if ((self_test_accel[0][0] - self_test_accel[1][0] < 1365) ||
            (self_test_accel[0][1] - self_test_accel[1][1] < 1365) ||
            (self_test_accel[0][2] - self_test_accel[1][2] < 680)) {
            return BMI088_SELF_TEST_ACCEL_ERROR;
        }

        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        return BMI088_NO_ERROR;
    };

    friend uint8_t Bmi088GyroTest(Bmi088* bmi088) {
        uint8_t res = 0;
        uint8_t retry = 0;

        // check commiunication is normal
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        // reset the gyro sensor
        bmi088->gyroWriteSingleReg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
        bsp::delay_ms(BMI088_LONG_DELAY_TIME);

        // check commiunication is normal after reset
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
        bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
        bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

        bmi088->gyroWriteSingleReg(BMI088_GYRO_SELF_TEST, BMI088_GYRO_TRIG_BIST);
        bsp::delay_ms(BMI088_LONG_DELAY_TIME);

        do {
            bmi088->gyroReadSingleReg(BMI088_GYRO_SELF_TEST, &res);
            bsp::delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
            retry++;
        } while (!(res & BMI088_GYRO_BIST_RDY) && retry < 10);

        if (retry == 10) {
            return BMI088_SELF_TEST_GYRO_ERROR;
        }

        if (res & BMI088_GYRO_BIST_FAIL) {
            return BMI088_SELF_TEST_GYRO_ERROR;
        }

        return BMI088_NO_ERROR;
    };
};

namespace bsp {
namespace bmi088 {
struct InsDataType {
    degree_t roll;
    degree_t pitch;
    degree_t yaw;
    degree_t yaw_total_angle;
    scalar_t yaw_round;
    degree_t yaw_zero_drift;
    degree_t yaw_offset;
    degrees_per_second_t roll_speed;
    degrees_per_second_t pitch_speed;
    degrees_per_second_t yaw_speed;
    void set_yaw_offset(units::angle::degree_t angle) { yaw_offset = angle; }
};

InsDataType const& GetImuDataRef();
void Start();
void Update();
} // namespace bmi088
} // namespace bsp